#include<iostream>
#include "PNP.h"
using namespace std;
using namespace cv;
#include <opencv2/core/mat.hpp>
void help();

int main(int argc, char const *argv[])
{
    PNP pnp(false);
    /* 设置相机内参数和畸变参数 */
    cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<  1.2853517927598091e+03, 0., 3.1944768628958542e+02,
     0.,1.2792339468697937e+03, 2.3929354061292258e+02, 
     0., 0., 1.);
    cv::Mat distCoeffs = (cv::Mat_<double>(5,1) << -6.3687295852461456e-01, -1.9748008790347320e+00,
       3.0970703651800782e-02, 2.1944646842516919e-03, 0. );
    pnp.set_camera_params(cameraMatrix, distCoeffs);

    /*设置装甲板尺寸参数*/
    double length = 67.5;
    double width = 26.5;
    pnp.set_objpoint(length, width);
    
    try
    {
        pnp.ImageprocessAndPnP(argv[1]); 
        // pnp.ImageprocessAndPnP("../装甲板_1.avi");
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
        help();
        return -1;
    }
    
    cout << "程序运行成功，无报错" << endl;
    return 0;
}

void help()
{
    cout <<"请阅读README.md文档 需在命令行中输入视频的地址\n";
}